#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Image  # 或者其他你感兴趣的消息类型
import rosbag
from config import Config

def read_bag_file(bagfile):
    bag = rosbag.Bag(bagfile)
    # for topic, msg, t in bag.read_messages(topics=['/aliengo_gazebo/lowState/state']):
    #     # 处理图像数据
    #     print(t,msg.motorState,msg.imu)
    for topic, msg, t in bag.read_messages(topics=['/low_state_info']):
        # 处理图像数据
        # print(t,msg.motorState)
        # 输出关节名称和对应的q值
        for joint_name, index in Config.joint2MotorIndex.items():
                q_value = msg.motorCmd[index].q
                print(f"{joint_name}: {q_value:.2f}",end=" ")
        print("\n")
    bag.close()

if __name__ == '__main__':
    read_bag_file('/home/xjz/catkin_ws/src/my_get_model_state_pkg/scripts/recorded_data.bag')